/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/*LINTLIBRARY*/
/*PROTOLIB1*/

/*#include <math.h>*/
#include "main.h"
#include "mexcite.h"

/**************************************************************************
*
* ROUTINE
*               ModifiedExcitation
*
* FUNCTION
*               Modify the stochastic code book excitation gain
*
* SYNOPSIS
*               ModifiedExcitation(LPC_res, Adapt_res, scale)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       LPC_res         fxpt_16  i      LPC residual
*       Adapt_res       fxpt_16  i      Adaptive analysis residual
*       scale           fxpt_16  o      Stochastice codebook gain scale
*
*==========================================================================
*
* DESCRIPTION
*
*       Depending on the current system state, the stochastic code book
*       excitation is reduced to a level that is low enough to produce
*       positive perceptual effects, yet is high enough so as not to upset
*       the dynamics of the system.  The main effect of the method is that
*       during sustained voiced sounds, the excitation level is attenuated.
*       In unvoiced and transition regions the level is amplified to a
*       level slightly more than that of standard CELP.
*
*       The relative adaptive code book excitation component is
*       increased in voiced regions by decreasing the stochastic code book
*       excitation component.  The amount of decrease in the stochastic
*       component depends on the efficiency of the adaptive component.
*       More reconstruction burden is placed on the adaptive component as
*       its efficiency increases.  The efficiency is measured by the
*       closeness (in the squareroot crosscorrelation sense) of the residual
*       signals before and after pitch prediction.  When the efficiency is
*       high (e.g., > 0.9), the stochastic component is amplified slightly
*       (e.g., one quantizer level).
*
*       The procedure for modifying the stochastic gain outside the
*       search loop is:
*       1)  Measure the efficiency of the adaptive component (ccor)
*       2)  Search the stochastic code book for the optimum codeword
*       3)  Modify the stochastic code book gain
*
*       This method is compatible with Federal Standard 1016.
*
*       This code represents the modified excitation work done by R556 which
*       was inspired by the constrained excitation work done by Yair Shoham.
*
*
*==========================================================================
*
* CALLED BY
*
*       Analysis
*
*==========================================================================
*
* REFERENCES
*
*       Shoham, Yair, "Constrained-Stochastic Excitation Coding of Speech
*       at 4.8 kbps," in Advances in Speech Coding, ed. B. Atal, V.
*       Cuperman, and A. Gersho, submitted to Kluwer Academic Publishers.
*
*       Shoham, Yair, "Constrained-Stochastic Excitation Coding of Speech,"
*       Abstracts of the IEEE Workshop on Speech Coding for
*       Telecommunications, 1989, p. 65.
*
***************************************************************************/
void ModifiedExcitation(
fxpt_16 LPC_res[RES_LEN],               /* 15.0 format */
fxpt_16 Adapt_res[RES_LEN],             /* 15.0 format */
fxpt_16 *scale)                         /* 3.12 format */
{
        fxpt_64 ccor, e1;
        int     i;
		fxpt_16 *pl, *pa;
		fxpt_32 acc;

FXPT_PUSHSTATE("ModifiedExcitation", -1.0, -1.0);
        /* Calculate the Euclidean norm of the LPC residual */
        /* Calculate the cross correlation of the LPC residual and the
         * Adaptive analysis residual
         */
        e1 = 1;
        ccor = 1;

        for (i=RES_LEN,pl=LPC_res,pa=Adapt_res; i>0; --i,++pl,++pa) {
                /*e1 += LPC_res[i] * LPC_res[i];*/
                /*e1 = fxpt_mac32(e1, fxpt_shr16_round(LPC_res[i], 3),
                    fxpt_shr16_round(LPC_res[i], 3));*/
                /*e1 = fxpt_add32(e1, fxpt_shr32_fast(
                    fxpt_mult32(*pl, *pl), 6));*/
                e1 += *pl* *pl;
                /*ccor += Adapt_res[i] * LPC_res[i];*/
                /*ccor = fxpt_mac32(ccor, fxpt_shr16_round(Adapt_res[i], 3),
                    fxpt_shr16_round(LPC_res[i], 3));*/
                /*ccor = fxpt_add32(ccor, fxpt_shr32_fast(
                    fxpt_mult32(*pa, *pl), 6));*/
                ccor += *pa * *pl;
        }

        /* Normalize the cross correlation */
        /*ccor /= e1;*/
		/* with RES_LEN=60 the maximum el and corr can have 6 significant bits >32,
			so, shift 6 and trunc is a safe operation */
        acc = fxpt_div32((fxpt_32)(ccor>>6), (fxpt_32)(e1>>6), 24);                 /* 7.24 format */

        /* Square root cross correlation scaling */
        /**scale = sqrt(fabs((double)(ccor)));*/
        *scale = integer_sqrt(fxpt_abs32(acc));        /* 19.12 format */

        /* Modify scale */
        if (*scale < 819)               /* 0.2 in 19.12 format */
                *scale = 819;           /* 0.2 in 19.12 format */
        else
                if (*scale >= 3686)     /* 0.9 in 19.12 format */
                        /**scale *= 1.4;*/
                        *scale = fxpt_saturate16_round( fxpt_mult32(*scale, 5734), 13 );

        /* *scale should now be in 3.12 format */

FXPT_POPSTATE();
}
